import jade.semantics.lang.sl.grammar.Term;
import iRobotCreate.Controller;
import iRobotCreate.IRobotState;
import iRobotCreate.LineFollower;
import iRobotCreate.iRobotCommands;
import iRobotCreate.iRobotCommands.Sensor;
import casa.LispAccessible;
import casa.ML;
import casa.URLDescriptor;
import casa.abcl.ParamsMap;
import casa.conversation2.SubscribeClientConversation;
import casa.event.TimeEvent;
import casa.ui.AgentUI;


public class myLittleiRobot extends Controller {

	static {
		  createCasaLispOperators(myLittleiRobot.class);
	  }
	
	public myLittleiRobot(ParamsMap params, AgentUI ui) throws Exception {
		super(params, ui);
	}
	
	@Override
	public void initializeAfterRegistered(boolean registered) {
		super.initializeAfterRegistered(registered);
		 
		try{
			
			@SuppressWarnings("unused")
			SubscribeClientConversation convBump = new SubscribeClientConversation(
					"--subscription-request", 
					this, getServer(), 
					"(all ?x (BumpsAndWheelDrops ?x))", null)
			{
				@Override
				public void update(URLDescriptor agentB, Term term) {
					if (term==null || demoOn())
						return;
					String intString = term.toString();
					int val = Integer.parseInt(intString);
					onBumpsAndWheelDrops(val);
				}
			};
				
			@SuppressWarnings("unused")
			SubscribeClientConversation convWall = new SubscribeClientConversation(
					"--subscription-request", 
					this, getServer(), 
					"(all ?x (Wall ?x))", null)
			{
				@Override
				public void update(URLDescriptor agentB, Term term) {
					if (term==null || demoOn())
						return;
					String intString = term.toString();
					int val = Integer.parseInt(intString);
					onWall(val);
				}
			};				
							
			@SuppressWarnings("unused")
			SubscribeClientConversation convVirtWall = new SubscribeClientConversation(
					"--subscription-request", 
					this, getServer(), 
					"(all ?x (VirtualWall ?x))", null)
			{
				@Override
				public void update(URLDescriptor agentB, Term term) {
					if (term==null || demoOn())
						return;
					String intString = term.toString();
					int val = Integer.parseInt(intString);
					onVirtWall(val);
				}
			};
			
		}
		catch (Throwable e)
		{
			println("error", "Controller.initializeAfterRegistered().SubscribeConversation", e);
		}
		setState(wandering);//Set initial state here.
	} 
	
	IRobotState wandering = new IRobotState("wandering"){
		@Override public void enterState(){
			sendMessage(ML.REQUEST, ML.EXECUTE, getServer() 
					  ,ML.LANGUAGE, "lisp"
					  ,ML.CONTENT, "(irobot.drive 400 :flush T)"
					  );
		}
		@Override public void handleEvent(Sensor sensor, short reading){
			switch(sensor){
			case BumpsAndWheelDrops:
				int deg = 0;
				switch (reading & 3) { //determine which bump sensor(s) where triggered (if any)
				case 0: // no bumps
					deg = 0;
					break;
				case 1: // right
					deg = 45;
					break;
				case 2: // left
					deg = -70;
					break;
				case 3: // both
					deg = 70;
					break;
				default:
					deg = 30;
					break;
				}
				// back up a bit to clear the obstacle and make the appropriate turn, and continue
				if (deg!=0) {
				  sendMessage(ML.REQUEST, ML.EXECUTE, getServer() 
						  ,ML.LANGUAGE, "lisp"
						  ,ML.CONTENT, "(agent.async '(progn () (irobot.drive 0 :flush T) (irobot.moveby -50) (irobot.rotate-deg "+deg+") (irobot.drive "+defSpeed+")))"
						  );
				}
				break;
			case Overcurrents:
				
				break;
			default:
				break;
			}
		}
	};
	
	
	boolean lightOn = false;
	@Override
	protected void onBumpsAndWheelDrops(int val) {
 		getCurrentState().handleEvent(Sensor.BumpsAndWheelDrops, (short)val);
  }
	


	protected void onWall(int val){
		getCurrentState().handleEvent(Sensor.Wall, (short)val);
	}

	protected void onVirtWall(int val){
		getCurrentState().handleEvent(Sensor.VirtualWall, (short)val);
	}
}


//Override the Controller's methods that are called from subscriptions (previous slides)
	//Add new subscriptions (for Wall, BumpsAndWheels, etc.)
	//Your subscriptions should call currentState.handleEvent()
	
	//In your initilizeAfterRegistered() call setState() to get going
	//other than that, just define your states:
	//IRobotState x = new IRobotState(X) {}
	
	//Avoid bugs:
	//Use IRobotState.enterState()
	//enterState() is always called every time you enter a state by calling Controller.setState()
	//If your state is something like "wandering", and assumes a precondition like "the robot is
	// moving", then make that precondition true by putting a drive command in enterState().
	// Don't rely on the precondition being true!
	// Use timer events
	
	
	//If you use a timed event, don't forget to cancel() if you take action before it comes due.
	/*alignSpinTimer = new TimeEvent(ML.EVENT, this, System.currentTimeMillis()+timetoCircle{
	@Override
	public void fireEvent(){
		super.fireEvent();
		setState(wandering);
		sendMessage(ML.REQUEST, ML.EXECUTE, server, ML.LANGUAGE, "lisp", ML.CONTENT, "(irobot.drive \"+defSpeed+\")");
	}
	};
	alignSpintTimer.start();*/
